`timescale 1ns / 1ns

module L_0_get(
    input   wire            sys_clk     ,
    input   wire            reset_n     ,
    input   wire            begin_flag  ,

    input   wire    [7:0]   x_in        , // 8位有符号数
    input   wire    [7:0]   y_in        , // 8位无符号数
    input   wire            z_in        , // 1 抓捕 0 抬起
    input   wire            d_in        , // 目的地

    output  reg     [23:0]  seta_1_out  ,
    output  reg     [23:0]  seta_2_out  ,
    output  reg     [23:0]  seta_3_out  ,
    output                  dir         ,

    output  reg             finish_flag
);

// 以下都是 1:1:14
/*
在之前的测试中，使用a_h_b_in = 24'd2659802（也就是104 / 164）
以及b_in = 24'd41也就是（164mm / 4）
这样能够提高机械臂伸展的距离，防止其因为机械问题向下倾斜
*/
// 定点数 1符号位 1整数 14小数位 37.73+81.14+7
parameter    a_h_b_in   = 24'd2726297               ;   // 104   / 160
parameter    h_in       = 24'd157286                ;   // 6     / 160
parameter    b_in       = 24'd40                    ;   // 160mm / 4
parameter    PI         = 24'd5898240               ;   // 1:7:8 90
parameter    four_dig   = 24'd3145728               ;   // 50

parameter   S1         =  4'b0000;
parameter   S2         =  4'b0001;
parameter   S3         =  4'b0011;
parameter   S4         =  4'b0010;
parameter   S5         =  4'b0110;
parameter   S6         =  4'b0111;
parameter   S7         =  4'b0101;
parameter   S8         =  4'b0100;
parameter   S9         =  4'b1100;
parameter   S10        =  4'b1101;
parameter   S11        =  4'b1111;
parameter   S12        =  4'b1110;

/* 主控 */
reg  [3:0]    central_reg       ;
reg           atan_begin_flag   ;
reg           div_begin_flag    ;
reg           fast_begin_flag   ;

wire           div_out_flag;
wire          atan_out_flag;
reg           fast_out_flag;

/* 开根号模块 */
reg           mode     ;

/* sqrt atan cos sin模块 */
reg  [23:0]   atan_in_x; // 1:1:22
reg  [23:0]   atan_in_y; // 1:1:22
reg  [23:0]   atan_in_z; // 1:1:22
wire [23:0]   atan_x_out;  // 1:1:22
wire [47:0]   atan_x_out_jiao;  //
wire [23:0]   atan_y_out;  // 1:1:22
wire [23:0]   atan_z_out;  // 1:7:16
/* 除法模块 */
reg  [23:0]	    div_a_in;
reg  [23:0]     div_b_in;
wire [23:0]	    div_y_out;

/* 迭代 */
reg         [4:0]       times       ; // 迭代次数
reg         [4:0]       addr_r      ; // 地址
reg         [23:0]      setan_r     ;
wire   		[23:0]      angle       ; // 每一次对应的角度

/* 中间变量 */
wire [23:0]   l_cha;
wire [7:0]    x_zheng;

// 数据转换(优化区域)
assign dir          = x_in[7]; // 可优化
assign x_zheng      = dir ? (-x_in) : x_in;
assign atan_x_out_jiao = atan_x_out * 24'd2547003; // 优化
assign l_cha = {div_y_out[17:0],5'b0} - atan_x_out;

/*
S1,2 计算x y
S3   计算l / jiaozheng 使用平方模式
S4   使用乘法器计算l
S5   调用atan计算角度seta1
S6   迭代初始化
S7   计算cos和sin
S8   计算距离
S9   进行比较，确定下一迭代值
S10  迭代结束，使用atan计算seta3
*/

/* 总控 */
always@(posedge sys_clk or negedge reset_n) begin
    if(reset_n == 0) begin
        central_reg     <= 4'd0;
        finish_flag     <= 1'b0;
        // sqrt atan cos sin
        mode            <= 1'b0;
        atan_begin_flag <= 1'b0;
        atan_in_x       <= 24'd0;
        atan_in_y       <= 24'd0;
        atan_in_z       <= 24'd0;
        // out
        seta_1_out      <= 24'd0;
        seta_2_out      <= 24'd0;
        seta_3_out      <= 24'd0;
        // div
        div_begin_flag  <= 1'b0;
        div_a_in        <= 24'd0;
        div_b_in        <= 24'd0;
        // reg
        fast_begin_flag <= 1'b0;
        // deidai
        times           <= 5'd0;
        addr_r          <= 5'd0;
        setan_r         <= 24'd0;
    end else if(begin_flag|
                atan_out_flag|
                fast_out_flag|
                div_out_flag ) begin
        casex (central_reg)
            S1: begin //  计算 L_reg
                mode      <= 1'b0            ; // 圆周模式
                atan_in_x <= y_in    << 15   ; // 不是16的原因是害怕溢出
                atan_in_y <= x_zheng << 15   ;

                atan_begin_flag <= 1'b1;
                central_reg <= S2;
            end
            S2: begin //  计算 L / b
                seta_1_out <= atan_z_out;

                div_a_in <= atan_x_out_jiao[45:22];
                div_b_in <= b_in;
                
                div_begin_flag <= 1'b1;
                central_reg <= S3;
            end
            S3: begin // 迭代初始化
                setan_r <= angle         ;
                times   <= 1'b0          ;
                addr_r  <= 1'b1          ;
                fast_begin_flag <= 1'b1  ;
                central_reg     <= S4    ;
            end
            S4: begin // 计算cos和sin
                mode      <= 1'b1   ; //
                atan_in_z <= setan_r;

                atan_begin_flag <= 1'b1;
                central_reg <= S5;
            end
            S5: begin // 计算距离
                mode  <= 1'b0; // 旋转模式
                atan_in_x = atan_y_out - a_h_b_in + h_in; // 抬高
                if(l_cha[23]) atan_in_y = - l_cha; // 多余
                else atan_in_y = l_cha;
                
                atan_begin_flag <= 1'b1;
                central_reg <= S6;
            end
            S6: begin // 进行比较，确定下一迭代值
                if(times < 5'd23) begin
                    if(atan_x_out_jiao[45:22] > a_h_b_in) begin // 角度太大
                        setan_r <= setan_r - angle;
                    end else begin
                        setan_r <= setan_r + angle;
                    end
                    addr_r <= addr_r + 1'b1;
                    times  <= addr_r;
                    central_reg <= S4; // 继续迭代
                end else begin
                    central_reg <= S7;
                end
                fast_begin_flag <= 1'b1;
            end
            S7: begin // 得到结果
                addr_r <= 5'b0; // 地址清零
                if(z_in && d_in) seta_2_out <= atan_z_out - (four_dig >> 3); // 松力
                else if(z_in && !d_in) seta_2_out <= atan_z_out + (PI >> 4); // 施力
                else seta_2_out <= atan_z_out - (four_dig >> 1); // 常规

                seta_3_out <= PI - atan_z_out + setan_r - four_dig;

                finish_flag <= 1'b1;
                central_reg <= S1;
            end
            default: central_reg <= S1;
        endcase
    end else begin
        atan_begin_flag <= 1'b0;
        div_begin_flag  <= 1'b0;
        fast_begin_flag <= 1'b0;
        finish_flag     <= 1'b0;

        central_reg     <= central_reg;
        mode            <= mode            ;
        atan_in_x       <= atan_in_x       ;
        atan_in_y       <= atan_in_y       ;
        atan_in_z       <= atan_in_z       ;
        seta_1_out      <= seta_1_out      ;
        seta_2_out      <= seta_2_out      ;
        seta_3_out      <= seta_3_out      ;
        div_a_in        <= div_a_in        ;
        div_b_in        <= div_b_in        ;
        times           <= times           ;
        addr_r          <= addr_r          ;
        setan_r         <= setan_r         ;
    end
end

// 快速
always@(posedge sys_clk or negedge reset_n) begin
    if(reset_n == 1'b0) begin
        fast_out_flag <= 1'b0;
    end else if(fast_begin_flag) begin
        fast_out_flag <= 1'b1;
    end else
        fast_out_flag <= 1'b0;
end

// 除法模块
div div_inst
(
    .quotient   (div_y_out      ),
    .remainder  (               ),
    .flag       (div_out_flag   ),
    .dividend   (div_a_in       ),
    .divider    (div_b_in       ),
    .start      (div_begin_flag ),
    .clk        (sys_clk        )
);

// 反正切函数模块
my_atan my_atan
(
    .CLK_50M     ( sys_clk           ),
    .RST_N       ( reset_n           ),
    .be_flag     ( atan_begin_flag   ),
    .x_data      ( atan_in_x         ),
    .y_data      ( atan_in_y         ),
    .t_data      ( atan_in_z         ),
    .mode        ( mode              ),
    .x_out       ( atan_x_out        ),
    .y_out       ( atan_y_out        ),
    .z_out       ( atan_z_out        ),
    .flag        ( atan_out_flag     )
);

rom_l_get	rom_l_get_inst
(
	.address    ( addr_r  ),
	.clock      ( sys_clk ),
	.q          ( angle   )
);

endmodule
